package packageLego;

import lejos.nxt.*;
import lejos.nxt.addon.EOPD;

public class Mouvement {
	public static void main(String[] args) throws InterruptedException {
		
		UltrasonicSensor captGauche= new UltrasonicSensor(SensorPort.S1);
		UltrasonicSensor captDroit= new UltrasonicSensor(SensorPort.S4);
		
		Motor.A.setSpeed(250);// 2 RPM
		Motor.B.setSpeed(250);// 2 RPM
		
		while(!Button.ESCAPE.isPressed())
			{
			LCD.drawString("Droite :"+captDroit.getDistance(),0,0 );
			LCD.drawString("Gauche:"+captGauche.getDistance(),1,1 );
			LCD.drawString("A:"+Motor.A.getSpeed()+" B:"+Motor.B.getSpeed(),2,2);
				LCD.refresh();
	 		   Motor.A.forward();
	 		  Motor.B.forward();
	 		 if(captGauche.getDistance()<=20 && captDroit.getDistance()<=20)
	 		 {
	 			 Motor.A.backward();
	 			 Motor.B.backward();
	 		 }
	 		 if(captGauche.getDistance()<=20)
	 		   {
	 			   Motor.B.rotateTo(360);
	 			   Motor.A.setSpeed(400);
	 		   }
	 		
	 		 else if(captDroit.getDistance()<=20)
	 		   {
	 			  Motor.A.rotateTo(360);
	 			  Motor.B.setSpeed(400);
	 		   }
	 		else
	 		{
	 			  Motor.A.setSpeed(250);
	 			  Motor.B.setSpeed(250);
	 		   	  Motor.A.forward();
	 			  Motor.B.forward();
	 		}
	 		   
	 		   
			}
	}
	
	

}
